#pragma once

#include "../../nclgl/Quaternion.h"
#include "../../nclgl/Vector3.h"
#include "../../nclgl/Matrix4.h"
#include "../../nclgl/SceneNode.h"

enum PhysicsNodeType {
	REGULAR,
	SOFT_BODY_CHILD,
	SOFT_BODY_PARENT,
	EXPLOSION,
	EXPLOSION_HASH,
	AI,
	HEIGHTMAP_CHILD,
	HEIGHTMAP_PARENT,
	MISSILE_PLAYER,
	MISSILE_ENEMY,
	MISSILE_AI,
	PLAYER,
	ENEMY,
	POWER_UP_MISSILES
};

class CollisionVolume;
class CollisionAABB;
class PhysicsNode	{
public:
	PhysicsNode(PhysicsNodeType pnt = REGULAR);
	PhysicsNode(Quaternion orientation, Vector3 position, PhysicsNodeType pnt = REGULAR);
	~PhysicsNode(void);

	Vector3		GetPosition() const					{ return m_position;}
	Vector3		GetLinearVelocity()					{ return m_linearVelocity;}
	Vector3		GetLinearAcceleration()				{ return m_linearAcceleration;}
	void		SetPosition(Vector3 p)				{ m_position = p;}
	void		SetLinearVelocity(Vector3 v)		{ m_linearVelocity = v;}
	void		SetLinearAcceleration(Vector3 a)	{ m_linearAcceleration = a;}

	float		GetInverseMass()					{return m_invMass;}
	void		SetInverseMass(float m)				{m_invMass = m;}

	Quaternion	GetOrientation()					{ return m_orientation;}
	Vector3		GetAngularVelocity()				{ return m_angularVelocity;}
	Vector3		GetAngularAcceleration()			{ return m_angularAcceleration;}
	Matrix4		GetInverseInertia()					{ return m_invInertia;}
	void		SetOrientation(Quaternion o)		{ m_orientation = o;}
	void		SetAngularVelocity(Vector3 v)		{ m_angularVelocity = v;}
	void		SetAngularAcceleration(Vector3 a)	{ m_angularAcceleration = a;}
	void		SetInverseInertia(Matrix4 i)		{ m_invInertia = i;}
	Vector3		PhysicsNode::GetForwardDirection();

	Matrix4		BuildTransform();

	virtual void		Update(float msec);

	void		SetTarget(SceneNode *s)		{ target = s;}
	SceneNode*	GetTarget()					{ return target;}

	Vector3	GetForce()						{ return m_force;}
	Vector3	GetTorque()						{ return m_torque;}
	void	AddForce(Vector3 f)				{ m_force += f;}

	void	AddExplodingForce(Vector3 dir, float dist) { 
		m_force += dir*1000.0f*(100.0f/dist);
		explosionForce = 10000.0f*(100.0f/dist);
	}

	void	SetExplodingForce(float f)		{ explosionForce = 0; }
	float	GetExplodingForce()				{ return explosionForce; }
	void	SetColPenetration(float p)		{ colPenetration = p; }
	float	GetColPenetration()				{ return colPenetration; }

	void	SetForce(Vector3 f)				{ m_force = f;}
	void	SetTorque(Vector3 t)			{ m_torque = t;}

	bool	GetResting()			const	{ return resting;}
	void	SetResting(bool r)				{ resting = r;}
	void	SetStatic(bool s)				{ m_static = s;}
	bool	GetStatic()				const	{ return m_static;}
	void	SetDragCoefficient(float d)		{ dragcoefficient = d;}
	float	GetDragCoefficient()			{ return dragcoefficient; }

	PhysicsNodeType GetType()						{ return type;}
	void			SetType(PhysicsNodeType t)		{ type = t;}

	void	SetCollisionVolume(CollisionVolume* cv);
	CollisionVolume* GetCollisionVolume()	const	{ return colVol;}

	CollisionAABB* GetAABB()			const		{ return aabb;}
	void SetAABB(CollisionAABB* a)					{
		aabb = a;
	}

	void	SetFreeSpin(bool f) { freeSpin = f; }

	bool applyGravity;
	
	PhysicsNode* parent;

	void LookAt(Vector3 target){
		//Get the normalized vector from the node to Target
		Vector3 vectorTo = target-m_position;
		vectorTo.Normalise();

		//Straight ahead vector
		Vector3 localVector = GetForwardDirection();

		//Get the cross product as the axis of rotation
		Vector3 axis = Vector3::Cross(vectorTo, localVector);
		axis.Normalise();

		//Get the dot product to find the angle
		float angle = acos(Vector3::Dot(localVector, vectorTo));

		//Determine whether or not the angle is positive
		//Get the cross product of the axis and the local vector
		Vector3 thirdVect = Vector3::Cross(axis, localVector);
		
		//If the dot product of that and the local vector is negative, so is the angle
		if (Vector3::Dot(thirdVect, vectorTo) < 0){
			angle = -angle;
		}

		Quaternion axisAngle;

		float degrees = RadToDeg(angle);
		Quaternion nq = axisAngle.AxisAngleToQuaterion(axis, degrees);

		m_orientation = nq * m_orientation;
	}

protected:
	bool		m_static;
	Vector3		m_position;
	CollisionAABB*	aabb;
	PhysicsNodeType type;
	
	//<---------LINEAR-------------->
	float		m_invMass;
	Vector3		m_linearAcceleration;
	Vector3		m_linearVelocity;
	Vector3		m_force;

	//<----------ANGULAR--------------->
	Quaternion  m_orientation;
	Vector3		m_angularVelocity;
	Vector3		m_angularAcceleration;
	Vector3		m_torque;
	Matrix4     m_invInertia;

	SceneNode*		target;
	
	bool			resting;
	bool			freeSpin;
	float			drag;
	float			dragcoefficient;
	float			explosionForce;
	float			colPenetration;

	Vector3 calcSymplecticEuler(Vector3 position, Vector3 &velocity, Vector3 acceleration, float dt){
		velocity = velocity + acceleration * dt;
		return position + velocity * dt;
	}

	/*Vector3 calcVerletIntegration(Vector3 acceleration, float msec){
		Vector3 newPosition = m_position * (2 - drag) - m_prevPosition * (1-drag) + acceleration *msec*msec;
		m_prevPosition = m_position;
		m_position = newPosition;
	}*/

	//float collisionRadius;
	CollisionVolume* colVol;

};

